/*
 * File: Fu_RotSpd.c
 *
 * Code generated for Simulink model 'Fu_RotSpd'.
 *
 * Model version                  : 1.738
 * Simulink Coder version         : 8.2 (R2012a) 29-Dec-2011
 * TLC version                    : 8.2 (Dec 29 2011)
 * C/C++ source code generated on : Sun Jun 16 13:42:30 2013
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: ARM Compatible->ARM Cortex
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "Fu_RotSpd.h"
#include "Fu_RotSpd_private.h"

/* Named constants for Chart: '<S1>/Fu_RotSpd_ModeManager' */
#define Fu_RotSpd_IN_IDLE              ((uint8_T)1U)
#define Fu_RotSpd_IN_LO_SPD            ((uint8_T)2U)
#define Fu_RotSpd_IN_RUN               ((uint8_T)3U)
#define Fu_RotSpd_IN_WAIT              ((uint8_T)4U)

/* Named constants for Chart: '<S9>/Chart' */
#define Fu_RotSpd_IN_NotVal            ((uint8_T)1U)
#define Fu_RotSpd_IN_Val               ((uint8_T)2U)

/* Exported block signals */
int32_T RotSpd_HallSnsrSIHiRes;        /* '<S9>/Divide1' */
int32_T RotSpd_HallSnsrRPMHiRes;       /* '<S9>/Switch2' */
int16_T RotSpd_HallSnsrRPM;            /* '<S9>/Data Type Conversion1' */
uint16_T FuSpd_PosRot;                 /* '<S3>/Data Type Conversion' */
uint16_T RotSpd_PosRotHallSnsr;        /* '<S5>/Index Vector' */
uint8_T RotSpd_St;                     /* '<S1>/Fu_RotSpd_ModeManager' */
boolean_T RotSpd_FlgRotStall;          /* '<S10>/Relational Operator' */

/* Block signals (auto storage) */
BlockIO_Fu_RotSpd Fu_RotSpd_B;

/* Block states (auto storage) */
D_Work_Fu_RotSpd Fu_RotSpd_DWork;

/* Real-time model */
RT_MODEL_Fu_RotSpd Fu_RotSpd_M_;
RT_MODEL_Fu_RotSpd *const Fu_RotSpd_M = &Fu_RotSpd_M_;

/* Model step function */
void Fu_RotSpd_step(void)
{
  int16_T rtb_Abs;
  boolean_T rtb_loc_HallTrig;
  int32_T rtb_Y;
  uint16_T rtb_Switch2;
  int16_T rtb_Abs_0;

  /* RelationalOperator: '<S10>/Relational Operator' incorporates:
   *  Constant: '<S10>/Constant2'
   *  Memory: '<S10>/Memory'
   */
  RotSpd_FlgRotStall = (Fu_RotSpd_DWork.Memory_PreviousInput_i >= 500);

  /* Chart: '<S9>/Chart' incorporates:
   *  Inport: '<Root>/In2'
   */
  /* Gateway: Fu_RotSpd/RotSpd_Speed/Fu_RotSpd_CalcRotSpeed/Chart */
  /* During: Fu_RotSpd/RotSpd_Speed/Fu_RotSpd_CalcRotSpeed/Chart */
  if (Fu_RotSpd_DWork.is_active_c1_Fu_RotSpd == 0) {
    /* Entry: Fu_RotSpd/RotSpd_Speed/Fu_RotSpd_CalcRotSpeed/Chart */
    Fu_RotSpd_DWork.is_active_c1_Fu_RotSpd = 1U;

    /* Entry Internal: Fu_RotSpd/RotSpd_Speed/Fu_RotSpd_CalcRotSpeed/Chart */
    /* Transition: '<S11>:5' */
    Fu_RotSpd_DWork.is_c1_Fu_RotSpd = Fu_RotSpd_IN_NotVal;

    /* Entry 'NotVal': '<S11>:3' */
    Fu_RotSpd_B.val = 0U;
  } else {
    if ((Fu_RotSpd_DWork.is_c1_Fu_RotSpd == Fu_RotSpd_IN_NotVal) &&
        ((HallCalc_TickDifAry[1] < 16777217U) && (HallCalc_TickDifAry[2] <
          16777217U) && (HallCalc_TickDifAry[3] < 16777217U) &&
         (HallCalc_TickDifAry[4] < 16777217U) && (HallCalc_TickDifAry[5] <
          16777217U) && (HallCalc_TickDifAry[6] < 16777217U))) {
      /* During 'NotVal': '<S11>:3' */
      /* Transition: '<S11>:6' */
      Fu_RotSpd_DWork.is_c1_Fu_RotSpd = Fu_RotSpd_IN_Val;

      /* Entry 'Val': '<S11>:4' */
      Fu_RotSpd_B.val = 1U;
    }
  }

  /* End of Chart: '<S9>/Chart' */

  /* Switch: '<S9>/Switch2' incorporates:
   *  Switch: '<S9>/Switch1'
   */
  if (RotSpd_FlgRotStall) {
    RotSpd_HallSnsrRPMHiRes = 0;
  } else {
    if (Fu_RotSpd_B.val != 0U) {
      /* Switch: '<S9>/Switch1' incorporates:
       *  Inport: '<Root>/In1'
       *  Inport: '<Root>/In2'
       *  MultiPortSwitch: '<S9>/Index Vector'
       *  Product: '<S9>/Divide'
       *  Product: '<S9>/Divide2'
       *  Product: '<S9>/Divide3'
       *  Product: '<S9>/Divide4'
       *  Sum: '<S9>/Add'
       *  Sum: '<S9>/Add1'
       */
      RotSpd_HallSnsrRPMHiRes = mul_s32_s32_s32_sr17_sat
        (div_repeat_sus32_sat_floor(2304000000U, mul_s32_s32_s32_sr4_sat
          (div_repeat_s32_sat_floor(mul_s32_s32_s32_sr4_sat
            (div_repeat_sus32_sat_floor(((((HallCalc_TickDifAry[1] +
                  HallCalc_TickDifAry[2]) + HallCalc_TickDifAry[3]) +
                HallCalc_TickDifAry[4]) + HallCalc_TickDifAry[5]) +
              HallCalc_TickDifAry[6], 15, 3U), 25) + mul_s32_s32_s32_sr4_sat
            (div_repeat_sus32_sat_floor(HallCalc_TickDifAry[HallCalc_IndexPlaus],
              5, 4U), 25), 15, 4U), 25), 19U), 78125);
    }
  }

  /* End of Switch: '<S9>/Switch2' */

  /* DataTypeConversion: '<S9>/Data Type Conversion1' */
  RotSpd_HallSnsrRPM = (int16_T)mul_s32_s32_s32_sr42(RotSpd_HallSnsrRPMHiRes,
    439804651);

  /* Product: '<S9>/Divide1' incorporates:
   *  Constant: '<S9>/Constant3'
   */
  RotSpd_HallSnsrSIHiRes = mul_s32_s32_s32_sr28_sat(mul_s32_s32_s32_sr10_sat
    (RotSpd_HallSnsrRPMHiRes, 1047), 274877907);

  /* Abs: '<S1>/Abs' */
  if (RotSpd_HallSnsrRPM < 0) {
    rtb_Abs = (int16_T)-RotSpd_HallSnsrRPM;
  } else {
    rtb_Abs = RotSpd_HallSnsrRPM;
  }

  /* End of Abs: '<S1>/Abs' */

  /* Chart: '<S1>/Fu_RotSpd_ModeManager' incorporates:
   *  Inport: '<Root>/In6'
   */
  /* Gateway: Fu_RotSpd/Fu_RotSpd_ModeManager */
  /* During: Fu_RotSpd/Fu_RotSpd_ModeManager */
  if (Fu_RotSpd_DWork.is_active_c2_Fu_RotSpd == 0) {
    /* Entry: Fu_RotSpd/Fu_RotSpd_ModeManager */
    Fu_RotSpd_DWork.is_active_c2_Fu_RotSpd = 1U;

    /* Entry Internal: Fu_RotSpd/Fu_RotSpd_ModeManager */
    /* Transition: '<S2>:4' */
    Fu_RotSpd_DWork.is_c2_Fu_RotSpd = Fu_RotSpd_IN_IDLE;

    /* Entry 'IDLE': '<S2>:1' */
    RotSpd_St = 0U;
    Fu_RotSpd_B.loc_FlgCalReq = FALSE;
  } else {
    switch (Fu_RotSpd_DWork.is_c2_Fu_RotSpd) {
     case Fu_RotSpd_IN_IDLE:
      /* During 'IDLE': '<S2>:1' */
      if (Coord_FlgRunReq) {
        /* Transition: '<S2>:5' */
        Fu_RotSpd_DWork.is_c2_Fu_RotSpd = Fu_RotSpd_IN_LO_SPD;

        /* Entry 'LO_SPD': '<S2>:2' */
        RotSpd_St = 1U;
        Fu_RotSpd_B.loc_FlgCalReq = TRUE;
      }
      break;

     case Fu_RotSpd_IN_LO_SPD:
      /* During 'LO_SPD': '<S2>:2' */
      if (!Coord_FlgRunReq) {
        /* Transition: '<S2>:6' */
        Fu_RotSpd_DWork.is_c2_Fu_RotSpd = Fu_RotSpd_IN_IDLE;

        /* Entry 'IDLE': '<S2>:1' */
        RotSpd_St = 0U;
        Fu_RotSpd_B.loc_FlgCalReq = FALSE;
      } else {
        if (rtb_Abs > 900) {
          /* Transition: '<S2>:8' */
          Fu_RotSpd_DWork.loc_cntr = 0U;
          Fu_RotSpd_DWork.is_c2_Fu_RotSpd = Fu_RotSpd_IN_WAIT;

          /* Entry 'WAIT': '<S2>:15' */
          rtb_Y = Fu_RotSpd_DWork.loc_cntr + 1;
          if (rtb_Y > 65535) {
            rtb_Y = 65535;
          }

          Fu_RotSpd_DWork.loc_cntr = (uint16_T)rtb_Y;
        }
      }
      break;

     case Fu_RotSpd_IN_RUN:
      /* During 'RUN': '<S2>:3' */
      if (!Coord_FlgRunReq) {
        /* Transition: '<S2>:7' */
        Fu_RotSpd_DWork.is_c2_Fu_RotSpd = Fu_RotSpd_IN_IDLE;

        /* Entry 'IDLE': '<S2>:1' */
        RotSpd_St = 0U;
        Fu_RotSpd_B.loc_FlgCalReq = FALSE;
      } else {
        if (rtb_Abs < 800) {
          /* Transition: '<S2>:9' */
          Fu_RotSpd_DWork.is_c2_Fu_RotSpd = Fu_RotSpd_IN_LO_SPD;

          /* Entry 'LO_SPD': '<S2>:2' */
          RotSpd_St = 1U;
          Fu_RotSpd_B.loc_FlgCalReq = TRUE;
        }
      }
      break;

     default:
      /* During 'WAIT': '<S2>:15' */
      if (rtb_Abs < 800) {
        /* Transition: '<S2>:19' */
        Fu_RotSpd_DWork.is_c2_Fu_RotSpd = Fu_RotSpd_IN_LO_SPD;

        /* Entry 'LO_SPD': '<S2>:2' */
        RotSpd_St = 1U;
        Fu_RotSpd_B.loc_FlgCalReq = TRUE;
      } else if (Fu_RotSpd_DWork.loc_cntr > 10000) {
        /* Transition: '<S2>:16' */
        Fu_RotSpd_DWork.is_c2_Fu_RotSpd = Fu_RotSpd_IN_RUN;

        /* Entry 'RUN': '<S2>:3' */
        RotSpd_St = 2U;
        Fu_RotSpd_B.loc_FlgCalReq = FALSE;
      } else {
        /* Transition: '<S2>:18' */
        Fu_RotSpd_DWork.is_c2_Fu_RotSpd = Fu_RotSpd_IN_WAIT;

        /* Entry 'WAIT': '<S2>:15' */
        rtb_Y = Fu_RotSpd_DWork.loc_cntr + 1;
        if (rtb_Y > 65535) {
          rtb_Y = 65535;
        }

        Fu_RotSpd_DWork.loc_cntr = (uint16_T)rtb_Y;
      }
      break;
    }
  }

  /* End of Chart: '<S1>/Fu_RotSpd_ModeManager' */

  /* Chart: '<S6>/RateLimiter' incorporates:
   *  Constant: '<S3>/Constant5'
   *  Memory: '<S6>/Memory'
   *  Memory: '<S7>/Memory'
   *  RelationalOperator: '<S7>/Relational Operator'
   */
  /* Gateway: Fu_RotSpd/Fu_RotSpd_RotPosIntegrator/RateLimiter/RateLimiter */
  /* During: Fu_RotSpd/Fu_RotSpd_RotPosIntegrator/RateLimiter/RateLimiter */
  /* Entry Internal: Fu_RotSpd/Fu_RotSpd_RotPosIntegrator/RateLimiter/RateLimiter */
  /* Transition: '<S8>:8' */
  if ((int32_T)Fu_RotSpd_B.loc_FlgCalReq > (int32_T)
      Fu_RotSpd_DWork.Memory_PreviousInput_a) {
    /* Transition: '<S8>:10' */
    rtb_Y = RotSpd_HallSnsrSIHiRes;

    /* Transition: '<S8>:24' */
    /* Transition: '<S8>:25' */
    /* Transition: '<S8>:26' */
  } else {
    /* Transition: '<S8>:12' */
    rtb_Y = Fu_RotSpd_DWork.Memory_PreviousInput_j + 100;
    if ((Fu_RotSpd_DWork.Memory_PreviousInput_j > 0) && (rtb_Y <= 0)) {
      rtb_Y = MAX_int32_T;
    }

    if (rtb_Y < 12500000) {
      /* Transition: '<S8>:15' */
      rtb_Y = Fu_RotSpd_DWork.Memory_PreviousInput_j + 100;
      if ((Fu_RotSpd_DWork.Memory_PreviousInput_j > 0) && (rtb_Y <= 0)) {
        rtb_Y = MAX_int32_T;
      }

      /* Transition: '<S8>:25' */
      /* Transition: '<S8>:26' */
    } else {
      /* Transition: '<S8>:17' */
      rtb_Y = Fu_RotSpd_DWork.Memory_PreviousInput_j;
      if ((Fu_RotSpd_DWork.Memory_PreviousInput_j < 0) &&
          (Fu_RotSpd_DWork.Memory_PreviousInput_j >= 0)) {
        rtb_Y = MIN_int32_T;
      }

      if (rtb_Y > 12500000) {
        /* Transition: '<S8>:19' */
        rtb_Y = Fu_RotSpd_DWork.Memory_PreviousInput_j;
        if ((Fu_RotSpd_DWork.Memory_PreviousInput_j < 0) &&
            (Fu_RotSpd_DWork.Memory_PreviousInput_j >= 0)) {
          rtb_Y = MIN_int32_T;
        }

        /* Transition: '<S8>:26' */
      } else {
        /* Transition: '<S8>:21' */
        /* Transition: '<S8>:23' */
        rtb_Y = 12500000;
      }
    }
  }

  /* End of Chart: '<S6>/RateLimiter' */

  /* Switch: '<S3>/Switch1' incorporates:
   *  Memory: '<S5>/Memory'
   *  Product: '<S3>/Product1'
   */
  if (!Fu_RotSpd_B.loc_FlgCalReq) {
    rtb_Y = mul_s32_s32_s32_sr28_zero(mul_s32_s32_s32_sr10_zero
      (RotSpd_HallSnsrSIHiRes, (int32_T)Fu_RotSpd_DWork.Memory_PreviousInput_n),
      274877907);
  }

  /* End of Switch: '<S3>/Switch1' */

  /* Switch: '<S3>/Switch3' incorporates:
   *  Memory: '<S3>/Memory'
   *  RelationalOperator: '<S3>/Relational Operator'
   *  RelationalOperator: '<S3>/Relational Operator1'
   *  Sum: '<S3>/Add2'
   *  Switch: '<S3>/Switch2'
   */
  if (Fu_RotSpd_DWork.Memory_PreviousInput < 0) {
    rtb_Switch2 = (uint16_T)mul_s32_s32_s32_sr42
      (Fu_RotSpd_DWork.Memory_PreviousInput + 628320000, 439804651);
  } else if (Fu_RotSpd_DWork.Memory_PreviousInput >= 628320000) {
    /* Switch: '<S3>/Switch2' incorporates:
     *  Sum: '<S3>/Add1'
     */
    rtb_Switch2 = (uint16_T)mul_s32_s32_s32_sr42
      (Fu_RotSpd_DWork.Memory_PreviousInput - 628320000, 439804651);
  } else {
    /* Switch: '<S3>/Switch2' */
    rtb_Switch2 = (uint16_T)mul_s32_s32_s32_sr42
      (Fu_RotSpd_DWork.Memory_PreviousInput, 439804651);
  }

  /* End of Switch: '<S3>/Switch3' */

  /* Sum: '<S3>/Add' incorporates:
   *  Product: '<S3>/Product'
   */
  rtb_Y = (int32_T)((uint32_T)rtb_Switch2 * 10000U) + mul_s32_s32_s32_sr33
    (mul_s32_s32_s32_sr28_zero(mul_s32_s32_s32_sr9_zero(rtb_Y, 569), 274877907),
     858993459);

  /* DataTypeConversion: '<S3>/Data Type Conversion' */
  FuSpd_PosRot = (uint16_T)mul_s32_s32_s32_sr42(rtb_Y, 439804651);

  /* MultiPortSwitch: '<S5>/Index Vector' incorporates:
   *  Constant: '<S5>/Constant6'
   *  Inport: '<Root>/In1'
   */
  RotSpd_PosRotHallSnsr = Fu_RotSpd_ConstP.Constant6_Value[HallCalc_IndexPlaus];

  /* Sum: '<S5>/Add3' */
  rtb_Abs = (int16_T)((int32_T)((uint32_T)RotSpd_PosRotHallSnsr * 52429U >> 19)
                      - (int32_T)((uint32_T)FuSpd_PosRot * 52429U >> 19));

  /* RelationalOperator: '<S4>/Relational Operator' incorporates:
   *  Inport: '<Root>/In1'
   *  Memory: '<S4>/Memory1'
   */
  rtb_loc_HallTrig = (Fu_RotSpd_DWork.Memory1_PreviousInput !=
                      HallCalc_IndexPlaus);

  /* Abs: '<S5>/Abs' */
  if (rtb_Abs < 0) {
    rtb_Abs_0 = (int16_T)-rtb_Abs;
  } else {
    rtb_Abs_0 = rtb_Abs;
  }

  /* End of Abs: '<S5>/Abs' */

  /* Switch: '<S5>/Switch' incorporates:
   *  Logic: '<S5>/Logical Operator'
   *  RelationalOperator: '<S5>/Relational Operator'
   */
  if ((rtb_Abs_0 <= 3142) && rtb_loc_HallTrig) {
    /* Sum: '<S5>/Add1' incorporates:
     *  Gain: '<S5>/Gain'
     */
    rtb_Abs = (int16_T)((3 * rtb_Abs >> 1) + 1000);

    /* Saturate: '<S5>/Saturation' */
    if (rtb_Abs >= 1400) {
      /* Update for Memory: '<S5>/Memory' */
      Fu_RotSpd_DWork.Memory_PreviousInput_n = 1400;
    } else if (rtb_Abs <= 600) {
      /* Update for Memory: '<S5>/Memory' */
      Fu_RotSpd_DWork.Memory_PreviousInput_n = 600;
    } else {
      /* Update for Memory: '<S5>/Memory' */
      Fu_RotSpd_DWork.Memory_PreviousInput_n = rtb_Abs;
    }

    /* End of Saturate: '<S5>/Saturation' */
  }

  /* End of Switch: '<S5>/Switch' */

  /* Switch: '<S10>/Switch' incorporates:
   *  Switch: '<S10>/Switch1'
   */
  if (rtb_loc_HallTrig) {
    /* Update for Memory: '<S10>/Memory' incorporates:
     *  Constant: '<S10>/Constant3'
     */
    Fu_RotSpd_DWork.Memory_PreviousInput_i = 0U;
  } else {
    if (RotSpd_FlgRotStall) {
      /* Switch: '<S10>/Switch1' incorporates:
       *  Constant: '<S10>/Constant'
       */
      rtb_Switch2 = 0U;
    } else {
      /* Switch: '<S10>/Switch1' incorporates:
       *  Constant: '<S10>/Constant1'
       */
      rtb_Switch2 = 1U;
    }

    /* Update for Memory: '<S10>/Memory' incorporates:
     *  Memory: '<S10>/Memory'
     *  Sum: '<S10>/Add'
     */
    Fu_RotSpd_DWork.Memory_PreviousInput_i = (uint16_T)((uint32_T)rtb_Switch2 +
      (uint32_T)Fu_RotSpd_DWork.Memory_PreviousInput_i);
  }

  /* End of Switch: '<S10>/Switch' */

  /* Update for Memory: '<S6>/Memory' incorporates:
   *  Constant: '<S3>/Constant5'
   */
  Fu_RotSpd_DWork.Memory_PreviousInput_j = 12500000;

  /* Update for Memory: '<S7>/Memory' */
  Fu_RotSpd_DWork.Memory_PreviousInput_a = Fu_RotSpd_B.loc_FlgCalReq;

  /* Update for Memory: '<S3>/Memory' */
  Fu_RotSpd_DWork.Memory_PreviousInput = rtb_Y;

  /* Update for Memory: '<S4>/Memory1' incorporates:
   *  Inport: '<Root>/In1'
   */
  Fu_RotSpd_DWork.Memory1_PreviousInput = HallCalc_IndexPlaus;
}

/* Model initialize function */
void Fu_RotSpd_initialize(void)
{
  /* Registration code */

  /* initialize error status */
  rtmSetErrorStatus(Fu_RotSpd_M, (NULL));

  /* block I/O */
  (void) memset(((void *) &Fu_RotSpd_B), 0,
                sizeof(BlockIO_Fu_RotSpd));

  /* exported global signals */
  RotSpd_HallSnsrSIHiRes = 0;
  RotSpd_HallSnsrRPMHiRes = 0;
  RotSpd_HallSnsrRPM = 0;
  FuSpd_PosRot = 0U;
  RotSpd_PosRotHallSnsr = 0U;
  RotSpd_St = 0U;
  RotSpd_FlgRotStall = FALSE;

  /* states (dwork) */
  (void) memset((void *)&Fu_RotSpd_DWork, 0,
                sizeof(D_Work_Fu_RotSpd));

  /* InitializeConditions for Chart: '<S9>/Chart' */
  Fu_RotSpd_DWork.is_active_c1_Fu_RotSpd = 0U;
  Fu_RotSpd_DWork.is_c1_Fu_RotSpd = 0U;
  Fu_RotSpd_B.val = 0U;

  /* InitializeConditions for Chart: '<S1>/Fu_RotSpd_ModeManager' */
  Fu_RotSpd_DWork.is_active_c2_Fu_RotSpd = 0U;
  Fu_RotSpd_DWork.is_c2_Fu_RotSpd = 0U;
  Fu_RotSpd_DWork.loc_cntr = 0U;
  RotSpd_St = 0U;
  Fu_RotSpd_B.loc_FlgCalReq = FALSE;
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */
